package org.bmo;

import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.TouchSensor;

public class OberArm {
	
    private static Motor armMotor;
    private static TouchSensor armSensor;
    private int defPower = 50;
    private int nullAngel = 0;
    private RoboticInterface ri;
    private boolean rotated = true;

	public OberArm(RoboticInterface roboticInterface, Motor m1, TouchSensor tS1) {
		
		ri=roboticInterface;
		armMotor=m1;
		armSensor=tS1;
		
		armMotor.setSpeed(50);
		armMotor.setPower(defPower);
	}	
	
	public boolean isOrigin(){
		return armSensor.isPressed();
	}

	public void moveToOrigin(){
		
		if(rotated){
				
			armMotor.setSpeed(10);
			
			while(!ri.isEscapeISPressed()&&isOrigin() != true){
				armMotor.forward();
				ri.setEscapeISPressed(Button.ESCAPE.isPressed());
			}
			
			nullAngel = armMotor.getTachoCount();
			armMotor.rotateTo(nullAngel+4);
			nullAngel = armMotor.getTachoCount();
			armMotor.setSpeed(50);
			rotated = false;
		}
	}
	
	public void rotateTo(int angel){
		armMotor.rotateTo(nullAngel+angel);
		rotated = true;
	}

	public int getPos(){
		return armMotor.getTachoCount()-nullAngel;
	}
	
	public void lock(int power){
		armMotor.lock(power);
	}
}
